#include "pathmap.h"

PathMap::PathMap(int width, int height)
    : width(width), height(height)
{
    isPassableMap = new bool[width * height];
    for (int i = 0; i < width * height; i++)
    {
        isPassableMap[i] = true;
    }
}

PathMap::~PathMap()
{
    delete[] isPassableMap;
}

float PathMap::LeastCostEstimate(void *stateStart, void *stateEnd)
{
    int startX, startY, endX, endY;
    StateToXY(stateStart, startX, startY);
    StateToXY(stateEnd, endX, endY);

    // Compute distance using the Manhattan method
    return abs((endX - startX) + (endY - startY));
}

void PathMap::AdjacentCost(void *state, std::vector<micropather::StateCost> *adjacent)
{
    int x, y;
    StateToXY(state, x, y);

    AddStateCostIfValid(x - 1, y - 1, 1.41f, adjacent);
    AddStateCostIfValid(x, y - 1, 1.0f, adjacent);
    AddStateCostIfValid(x + 1, y - 1, 1.41f, adjacent);
    AddStateCostIfValid(x - 1, y, 1.0f, adjacent);
    AddStateCostIfValid(x + 1, y, 1.0f, adjacent);
    AddStateCostIfValid(x - 1, y + 1, 1.41f, adjacent);
    AddStateCostIfValid(x, y + 1, 1.0f, adjacent);
    AddStateCostIfValid(x + 1, y + 1, 1.41f, adjacent);
}

void PathMap::PrintStateInfo(void *state)
{
    int x, y;
    StateToXY(state, x, y);
    printf("(%d,%d)", x, y);
}

void PathMap::SetPointPassable(int x, int y, bool isPassable)
{
    if (IsPointValid(x, y))
        isPassableMap[y * width + x] = isPassable;
}

bool PathMap::IsPointPassable(int x, int y)
{
    return isPassableMap[y * width + x];
}

void PathMap::StateToXY(void *state, int &x, int &y)
{
    int index = (int)state;
    y = index / width;
    x = index % width;
}

void* PathMap::XYToState(int x, int y)
{
    return (void*)(y * width + x);
}

bool PathMap::IsPointValid(int x, int y)
{
    return (x >= 0
            && x < width
            && y >= 0
            && y < height
            && isPassableMap[y * width + x]);
}

int PathMap::getWidth()
{
    return width;
}

int PathMap::getHeight()
{
    return height;
}

void PathMap::AddStateCostIfValid(int x, int y, float cost, std::vector<micropather::StateCost> *adjacent)
{
    if (IsPointValid(x, y))
    {
        StateCost stateCost;
        stateCost.state = XYToState(x, y);
        stateCost.cost = cost;
        adjacent->push_back(stateCost);
    }
}
